/*
 * tarm_runSolution.c
 * function: the kinematics and inverse kinematics
 *  s作用：顺解和逆解
 *  Created on: 2020年2月29日
 *      Author: AN
 */
#include <stdlib.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>

#include "TARM_Middleware/tarm_math.h"

#include "TARM_Application/tarm_run.h"


/************************************************************
*	四个函数：
*	xy_to_angle：坐标信息->角度信息，运算函数
*	xy_des_to_angle：坐标信息->角度信息，同步处理函数（包含栅格自动对齐）
*	angle_to_xy：角度信息->坐标信息，运算函数
*	angle_des_to_xy：角度信息->坐标信息，同步处理函数（包含栅格自动对齐）
*	传动比：69/12=5.75；69/9.5=7.263；1.8/16分
*	每一步度数：0.0195652173913=0.0196；0.01548946716232961586121437422553=0.0155
************************************************************/
void xy_to_angle(double x,double y,double* prime_angle,double* vice_angle)
{
//	double fai_t = asin(y / (sqrt(x*x + y*y))) * 180 / 3.14159;
//	double cita_t = acos(sqrt(x*x + y*y) / 40) * 180 / 3.14159;
	double fai_t = asin(y / tarm_sqrt(x*x + y*y)) * 180 / 3.14159;
	double cita_t = acos(tarm_sqrt(x*x + y*y) / 40) * 180 / 3.14159;
	*prime_angle = cita_t + fai_t;
	*vice_angle = cita_t - fai_t;
}


 void xy_des_to_angle(void)
{
    //1、计算对应角度
    double prime_a=0,vice_a=0;
    xy_to_angle(g_TARM_RunRecord.pv_distance.x, g_TARM_RunRecord.pv_distance.y, &prime_a, &vice_a);

    //2、以初始角度为基准对对应角度标准化处理
    g_TARM_RunRecord.pv_distance.p = g_TARM_Factor.null_p + ((int)((prime_a - g_TARM_Factor.null_p)/g_TARM_Factor.step_p))*g_TARM_Factor.step_p;
    g_TARM_RunRecord.pv_distance.v = g_TARM_Factor.null_v + ((int)((vice_a - g_TARM_Factor.null_v)/g_TARM_Factor.step_v))*g_TARM_Factor.step_v;
}

void angle_to_xy(double prime_angle,double vice_angle,double *x,double *y)
{
	double pri_ang_c,vic_ang_c,pri_ang_s,vic_ang_s;

//	pri_ang_c = cos(prime_angle/180*3.14)*g_TARM_Factor.length_p;
//	vic_ang_c = cos(vice_angle/180*3.14)*g_TARM_Factor.length_v;
//
//	pri_ang_s = sin(prime_angle/180*3.14)*g_TARM_Factor.length_p;
//	vic_ang_s = sin(vice_angle/180*3.14)*g_TARM_Factor.length_v;

	pri_ang_c = tarm_cos(prime_angle/180*3.14)*g_TARM_Factor.length_p;
	vic_ang_c = tarm_cos(vice_angle/180*3.14)*g_TARM_Factor.length_v;

	pri_ang_s = tarm_sin(prime_angle/180*3.14)*g_TARM_Factor.length_p;
	vic_ang_s = tarm_sin(vice_angle/180*3.14)*g_TARM_Factor.length_v;

	*x = pri_ang_c + vic_ang_c;
	*y = pri_ang_s - vic_ang_s;
}

void angle_des_to_xy()
{
    //1、以初始角度为基准对对应角度标准化处理
    g_TARM_RunRecord.pv_distance.p = g_TARM_Factor.null_p + ((int)((g_TARM_RunRecord.pv_distance.p - g_TARM_Factor.null_p)/g_TARM_Factor.step_p))*g_TARM_Factor.step_p;
    g_TARM_RunRecord.pv_distance.v = g_TARM_Factor.null_v + ((int)((g_TARM_RunRecord.pv_distance.v - g_TARM_Factor.null_v)/g_TARM_Factor.step_v))*g_TARM_Factor.step_v;

    //2、计算对应xy
    angle_to_xy(g_TARM_RunRecord.pv_distance.p, g_TARM_RunRecord.pv_distance.v,
                &g_TARM_RunRecord.pv_distance.x, &g_TARM_RunRecord.pv_distance.y);
}

void angle_cur_to_xy()
{
    //1、以初始角度为基准对对应角度标准化处理
    g_TARM_RunRecord.pv_current.p = g_TARM_Factor.null_p + ((int)((g_TARM_RunRecord.pv_current.p - g_TARM_Factor.null_p)/g_TARM_Factor.step_p))*g_TARM_Factor.step_p;
    g_TARM_RunRecord.pv_current.v = g_TARM_Factor.null_v + ((int)((g_TARM_RunRecord.pv_current.v - g_TARM_Factor.null_v)/g_TARM_Factor.step_v))*g_TARM_Factor.step_v;

    //2、计算对应xy
    angle_to_xy(g_TARM_RunRecord.pv_current.p, g_TARM_RunRecord.pv_current.v,
                &g_TARM_RunRecord.pv_current.x, &g_TARM_RunRecord.pv_current.y);
}


